/*
 * pid.c
 *
 * Created: 20241209
 * Author: Hecate
 */
#include "pid.h"

#include <math.h>
#include <stdlib.h>

struct PID
{
    float expectedValue;
    float lastError;
    float beforeLastError;
    float integral;
    float kp, ki, kd;
    float controlValue;

    float integralMax;

    void (*process)(PID *, float);
};

static void incrementalPidProcessHandler(PID *obj, float currentValue)
{
    // 误差计算
    float error = obj->expectedValue - currentValue;
    float deltaError = error - obj->lastError;

    // 增量累加为控制值
    obj->controlValue +=
        obj->kp * deltaError +
        obj->ki * error +
        obj->kd * (deltaError - (obj->lastError - obj->beforeLastError));

    // 误差存储
    obj->beforeLastError = obj->lastError;
    obj->lastError = error;
}

static void positionalPidHandler(PID *obj, float currentValue)
{
    // 误差计算
    float error = obj->expectedValue - currentValue;

    // 积分累加
    obj->integral += error;

    // 积分限幅
    if (obj->integralMax)
    {
        if (obj->integral > obj->integralMax) obj->integral = obj->integralMax;
        else if (obj->integral < -obj->integralMax) obj->integral = -obj->integralMax;
    }
    // 控制值计算
    obj->controlValue =
        obj->kp * error +
        obj->ki * obj->integral +
        obj->kd * (error - obj->lastError);

    // 误差存储
    obj->lastError = error;
}

void pidProcess(PID *obj, float currentValue)
{
    if (obj->process) obj->process(obj, currentValue);
}

PID *pidInit(float integralMax, float target, float kp, float ki, float kd)
{
    PID *obj = (PID *) malloc(sizeof(PID));

    // 根据积分限幅参数自动选择PID模式
    if (integralMax == 0) obj->process = incrementalPidProcessHandler;
    else obj->process = positionalPidHandler;

    // 设置控制参数
    obj->expectedValue = target;
    obj->kp = kp;
    obj->ki = ki;
    obj->kd = kd;

    if (ki != 0.0f && integralMax != 0.0f) obj->integralMax = fabsf(integralMax / ki);
    else obj->integralMax = 0.0f;

    // 初始化所有状态变量为0，确保干净的起始状态
    obj->lastError = 0;
    obj->beforeLastError = 0;
    obj->integral = 0;
    obj->controlValue = 0;

    return obj;
}

void pidSetTarget(PID *obj, float targetValue)
{
    obj->expectedValue = targetValue;
}

float pidGetControlValue(const PID *obj)
{
    return obj->controlValue;
}
